/*
 * Author: nicolas.bredeche(at)lri.fr
 * 2006/12/21
 * 
 * a Braitenberg robot demo using simbad (simbad 1.4+ required, may work with previous version)
 */
package piconode.tutorials;

import javax.vecmath.Vector3d;
import javax.vecmath.Vector3f;

import piconode.core.arc.WeightedArc;
import piconode.core.node.NeuralNetwork;
import piconode.core.node.Neuron;
import piconode.ext.ActivationFunction_LogisticSigmoid;
import simbad.gui.Simbad;
import simbad.sim.Agent;
import simbad.sim.Arch;
import simbad.sim.Box;
import simbad.sim.EnvironmentDescription;
import simbad.sim.LightSensor;
import simbad.sim.RobotFactory;
import simbad.sim.Wall;

/**
 * This demo shows how to capture the camera image , process it and display it
 * in dedicated window.
 * 
 * Warning! you may experience some funny results depending on your openGL
 * implementation (i.e. light sensor may note be implemented in your card openGL
 * driver) and locked openGL services (i.e. some external apps may have locked
 * some resources, making simbad unable to use them) - under linux, an easy
 * solution is to rely on the MESA openGL sofware implementation.
 * 
 */
public class Tutorial_6a_Robot_BraitenbergLightSearch extends EnvironmentDescription {

	public Tutorial_6a_Robot_BraitenbergLightSearch() {
		light1IsOn = true;
		light1SetPosition(-6, .7f, 5);
		light2IsOn = false;
		// light2SetPosition(0,.7f,0); // [TODO [!n] 200612242349 - le robot ne
		// voit pas la 2nde lampe...]
		ambientLightColor = darkgray;
		light1Color = white;
		light2Color = white;
		wallColor = blue;
		archColor = green;
		boxColor = red;
		floorColor = white;
		backgroundColor = black;
		hasAxis = true;
		Wall w1 = new Wall(new Vector3d(9, 0, 0), 19, 1, this);
		w1.rotate90(1);
		add(w1);
		Wall w2 = new Wall(new Vector3d(-9, 0, 0), 19, 2, this);
		w2.rotate90(1);
		add(w2);
		Wall w3 = new Wall(new Vector3d(0, 0, 9), 19, 1, this);
		add(w3);
		Wall w4 = new Wall(new Vector3d(0, 0, -9), 19, 2, this);
		add(w4);
		Box b1 = new Box(new Vector3d(-3, 0, -3), new Vector3f(1, 1, 2), this);
		add(b1);
		Arch a1 = new Arch(new Vector3d(3, 0, -3), this);
		add(a1);
		add(new Robot(new Vector3d(-3, 0, -5), "BraitenbergRobot", this));
	}

	class Robot extends Agent {
		LightSensor sensorLeft;
		LightSensor sensorRight;
		EnvironmentDescription _env;
		NeuralNetwork network;

		public Robot(Vector3d position, String name, EnvironmentDescription env) {
			super(position, name);
			sensorLeft = RobotFactory.addLightSensorLeft(this);
			sensorRight = RobotFactory.addLightSensorRight(this);
			this._env = env;
		}

		/** Initialize Agent's Behavior */
		@Override
		public void initBehavior() {

			// step 1 : build a Braitenberg net from the neural net class - both
			// are similar, the difference
			// is that the arcs provide either activation (+1) or inhibation
			// (-1).
			network = new NeuralNetwork();

			// step 2 : create neurons, the two inputs will be used as entry
			// point for light sensor values
			// note that we stick with the sigmoid activation function to bound
			// output values

			Neuron in1 = new Neuron(network, new ActivationFunction_LogisticSigmoid(), "sensorleftnode");
			Neuron in2 = new Neuron(network, new ActivationFunction_LogisticSigmoid(), "sensorrightnode");
			Neuron out1 = new Neuron(network, new ActivationFunction_LogisticSigmoid(), "motorleftnode");
			Neuron out2 = new Neuron(network, new ActivationFunction_LogisticSigmoid(), "motorrightnode");

			// step 3 : declare input and output neurons

			network.registerInputNeuron(in1);
			network.registerInputNeuron(in2);
			network.registerOutputNeuron(out1);
			network.registerOutputNeuron(out2);

			// step 4 : create the topology
			// This is here that we create the special Braitenberg net flavor.
			// In this example, light sensors are connected to opposite motor
			// activator, both connections are excitatory (weight=1).
			// if connection are in1->out1,in2->out2, the robot flee escape from
			// the light

			network.registerArc(new WeightedArc(in1, out2, 1.f));
			network.registerArc(new WeightedArc(in2, out1, 1.f));

			// step 5 : initialize the network (perform some integrity checks
			// and internal encoding)

			network.initNetwork();
			network.displayInformation();

		}

		/** Perform one step of Agent's Behavior */
		@Override
		public void performBehavior() {

			/**/
			// Braitenberg's code
			// step 1 : loading the input values
			network.getInputNeuron("sensorleftnode").setInputValue(sensorLeft.getAverageLuminance());
			network.getInputNeuron("sensorrightnode").setInputValue(sensorRight.getAverageLuminance());

			// step 2 : computing the output values -- (!) i.e. left motor and
			// right motor speed!
			network.step(); // assumes return values btw -1 and 1

			// step 3 : get motor value and convert to simbad kinematic
			// (translational and angular speeds rather than motor commands).
			double leftMotor = network.getOutputNeuron("motorleftnode").getValue();
			double rightMotor = network.getOutputNeuron("motorrightnode").getValue();

			setTranslationalVelocity((leftMotor + rightMotor) / 2.0);
			if (leftMotor < rightMotor) // check direction
				setRotationalVelocity(+Math.abs(leftMotor - rightMotor) * Math.PI); // remember:
			// anti-clockwise.
			else
				setRotationalVelocity(-Math.abs(leftMotor - rightMotor) * Math.PI);

			if (collisionDetected())
				moveToStartPosition();
			/**/

			/*******************************************************************
			 * // deterministic hand coded controler (old code) // progress at
			 * 0.5 m/s // turn towards light
			 * 
			 * float llum = sensorLeft.getAverageLuminance(); float rlum =
			 * sensorRight.getAverageLuminance();
			 * 
			 * //setRotationalVelocity((llum - rlum) * Math.PI);
			 * this.setTranslationalVelocity(0.5);
			 * this.setRotationalVelocity((llum - rlum) * Math.PI/4);
			 * 
			 * if (collisionDetected()) moveToStartPosition(); /
			 ******************************************************************/

			// changing light position when reached (do not work for now in
			// current simbad implementation
			/*
			 * Point3d coord = new Point3d(); this.getCoords(coord); if (
			 * Math.abs ( coord.x - this._env.light1Position.x ) <= 1 &&
			 * Math.abs ( coord.z - this._env.light1Position.z ) <= 1 ) {
			 * light1IsOn = false; light2IsOn = true; System.out.println("found
			 * light!"); }
			 */

		}
	}

	/*
	 * for test purposes
	 */
	public static void main(String[] args) {
		System.out.println("# Braitenberg tutorial - use Simbad and Piconode.\n# see Robot.initBehavior and Robot.performBehavior methods.\n-----------------");
		System.out.println("# (!!!) IMPORTANT NOTE: this demo may encounter problems if your openGL is not fully implemented and/or locked by other applications.");
		Tutorial_6a_Robot_BraitenbergLightSearch env = new Tutorial_6a_Robot_BraitenbergLightSearch();
		new Simbad(env, false);
	}
}
